#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/cloud_viewer.h>

typedef pcl::PointXYZRGB PointT;
// typedef pcl::PointXYZ PointT;

int main(int agrc, char** argv)
{   
    pcl::PointCloud<PointT>::Ptr cloud_src (new pcl::PointCloud<PointT>);
    pcl::PointCloud<PointT>::Ptr cloud_target (new pcl::PointCloud<PointT>);
    pcl::PointCloud<PointT>::Ptr cloud_in_after_icp (new pcl::PointCloud<PointT>);//输入点云进过icp匹配后的点云

    cloud_src->width = 50;
    cloud_src->height = 1;
    cloud_src->points.resize(cloud_src->width * cloud_src->height);
    
    cloud_target->width = 50;
    cloud_target->height =1;
    cloud_target->points.resize(cloud_target->width * cloud_target->height);

    for (size_t i = 0; i < cloud_src->size(); ++i)
    {
        cloud_src->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
        cloud_src->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
        cloud_src->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
        cloud_src->points[i].r =  255 * rand();
        cloud_src->points[i].g =  255 * rand();
        cloud_src->points[i].b =  255 * rand();
    
        cloud_target->points[i].x = cloud_src->points[i].x + 3;
        cloud_target->points[i].y = cloud_src->points[i].y + 3;
        cloud_target->points[i].z = cloud_src->points[i].z;
        cloud_target->points[i].r = 0;
        cloud_target->points[i].g = 255;
        cloud_target->points[i].b = 0;

    }

     //创建一个IterativeClosestPoint实例，使用的奇异值分解
    pcl::IterativeClosestPoint<PointT, PointT> icp;
    icp.setMaxCorrespondenceDistance(100);  //对应点间的最大距离(单位为m)
    icp.setMaximumIterations(100);//迭代次数已达到用户施加的最大迭代次数
    icp.setTransformationEpsilon(1e-6);// //两次变化矩阵之间的差值,（即两次位姿转换）之间的 epsilon（差异）小于用户施加的值
    icp.setEuclideanFitnessEpsilon(1e-6);//欧几里得平方误差的总和小于用户定义的阈值
    icp.setRANSACIterations(0);// 设置RANSAC运行次数    
    icp.setInputSource(cloud_src);
    icp.setInputTarget(cloud_target);
    //cloud_in_after_icp用来存储应用ICP算法之后的结果
    icp.align(*cloud_in_after_icp);  //变换后的源点云

    //如果变换前后点云正确Align的话（即变换点云通过刚性变换之后几乎和变换后点云完全重合）
    //则 icp.hasConverged() = 1 (true)，然后输出fitness得分和其他一些相关信息。
    //icp.getFitnessScore() 用于获取迭代结束后目标点云和配准后的点云的最近点之间距离的均值。
    std::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl;
    std::cout << std::fixed << icp.getFinalTransformation() << std::endl;//获得最后的变换矩阵

    for(size_t i = 0; i < cloud_in_after_icp->size(); ++i)
    {
        cloud_in_after_icp->points[i].b = 0;
        cloud_in_after_icp->points[i].g = 0;
        cloud_in_after_icp->points[i].r = 255;
    }
    

    *cloud_src += *cloud_target;
    *cloud_src += *cloud_in_after_icp;
    
    pcl::visualization::PCLVisualizer viewer("Viewer");
    viewer.setBackgroundColor(0, 0, 0);
    pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb1(cloud_src);
    viewer.addPointCloud(cloud_src, rgb1, "Viewer");
   
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "Viewer");
    // 设置坐标轴长度
    viewer.addCoordinateSystem(1, "Viewer");
    viewer.spin();

    return 0;
}